#include "quaternion.h"

Quaternion quaternion_create(double w, double x, double y, double z) {
    Quaternion q = {w, x, y, z};
    return q;
}

Quaternion quaternion_identity() {
    return quaternion_create(1.0, 0.0, 0.0, 0.0);
}

Quaternion quaternion_from_axis_angle(Vector3D axis, double angle) {
    Vector3D n = vector3d_normalize(axis);
    double half_angle = angle * 0.5;
    double sin_half = sin(half_angle);
    
    return quaternion_create(
        cos(half_angle),
        n.x * sin_half,
        n.y * sin_half,
        n.z * sin_half
    );
}

Quaternion quaternion_from_euler(double roll, double pitch, double yaw) {
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    
    return quaternion_create(
        cr * cp * cy + sr * sp * sy,
        sr * cp * cy - cr * sp * sy,
        cr * sp * cy + sr * cp * sy,
        cr * cp * sy - sr * sp * cy
    );
}

Quaternion quaternion_multiply(Quaternion q1, Quaternion q2) {
    return quaternion_create(
        q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
        q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
        q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
        q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
    );
}

Quaternion quaternion_conjugate(Quaternion q) {
    return quaternion_create(q.w, -q.x, -q.y, -q.z);
}

Quaternion quaternion_inverse(Quaternion q) {
    double mag_sq = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
    if (mag_sq < 1e-10) {
        return quaternion_identity();
    }
    
    Quaternion conj = quaternion_conjugate(q);
    return quaternion_create(
        conj.w / mag_sq,
        conj.x / mag_sq,
        conj.y / mag_sq,
        conj.z / mag_sq
    );
}

double quaternion_magnitude(Quaternion q) {
    return sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
}

Quaternion quaternion_normalize(Quaternion q) {
    double mag = quaternion_magnitude(q);
    if (mag < 1e-10) {
        return quaternion_identity();
    }
    
    return quaternion_create(
        q.w / mag,
        q.x / mag,
        q.y / mag,
        q.z / mag
    );
}

Vector3D quaternion_rotate_vector(Quaternion q, Vector3D v) {
    Quaternion q_vec = quaternion_create(0.0, v.x, v.y, v.z);
    Quaternion q_inv = quaternion_conjugate(q);
    
    // q * v * q^-1
    Quaternion result = quaternion_multiply(quaternion_multiply(q, q_vec), q_inv);
    
    return vector3d_create(result.x, result.y, result.z);
}

Quaternion quaternion_slerp(Quaternion q1, Quaternion q2, double t) {
    t = CLAMP(t, 0.0, 1.0);
    
    q1 = quaternion_normalize(q1);
    q2 = quaternion_normalize(q2);
    
    double dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
    
    // 确保最短路径
    if (dot < 0.0) {
        q2 = quaternion_create(-q2.w, -q2.x, -q2.y, -q2.z);
        dot = -dot;
    }
    
    if (dot > 0.9995) {
        // 线性插值避免除零
        return quaternion_normalize(quaternion_create(
            q1.w + t * (q2.w - q1.w),
            q1.x + t * (q2.x - q1.x),
            q1.y + t * (q2.y - q1.y),
            q1.z + t * (q2.z - q1.z)
        ));
    }
    
    double theta = acos(dot);
    double sin_theta = sin(theta);
    
    return quaternion_create(
        (q1.w * sin((1.0 - t) * theta) + q2.w * sin(t * theta)) / sin_theta,
        (q1.x * sin((1.0 - t) * theta) + q2.x * sin(t * theta)) / sin_theta,
        (q1.y * sin((1.0 - t) * theta) + q2.y * sin(t * theta)) / sin_theta,
        (q1.z * sin((1.0 - t) * theta) + q2.z * sin(t * theta)) / sin_theta
    );
}

Quaternion quaternion_from_to_rotation(Vector3D from, Vector3D to) {
    Vector3D from_norm = vector3d_normalize(from);
    Vector3D to_norm = vector3d_normalize(to);
    
    double dot = vector3d_dot(from_norm, to_norm);
    
    if (dot > 0.99999) {
        return quaternion_identity();
    }
    
    if (dot < -0.99999) {
        // 180度旋转
        Vector3D axis = vector3d_cross(from_norm, vector3d_i());
        if (vector3d_magnitude(axis) < 1e-6) {
            axis = vector3d_cross(from_norm, vector3d_j());
        }
        axis = vector3d_normalize(axis);
        return quaternion_from_axis_angle(axis, PI);
    }
    
    Vector3D axis = vector3d_normalize(vector3d_cross(from_norm, to_norm));
    double angle = acos(dot);
    
    return quaternion_from_axis_angle(axis, angle);
}

Vector3D quaternion_to_euler(Quaternion q) {
    q = quaternion_normalize(q);
    
    double sinr_cosp = 2.0 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
    double roll = atan2(sinr_cosp, cosr_cosp);
    
    double sinp = 2.0 * (q.w * q.y - q.z * q.x);
    double pitch = 0.0;
    if (fabs(sinp) >= 1.0) {
        pitch = copysign(PI / 2.0, sinp);
    } else {
        pitch = asin(sinp);
    }
    
    double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
    double yaw = atan2(siny_cosp, cosy_cosp);
    
    return vector3d_create(roll, pitch, yaw);
}

double quaternion_angle(Quaternion q1, Quaternion q2) {
    q1 = quaternion_normalize(q1);
    q2 = quaternion_normalize(q2);
    
    double dot = fabs(q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z);
    dot = CLAMP(dot, -1.0, 1.0);
    
    return 2.0 * acos(dot);
}

void quaternion_print(const char* name, Quaternion q) {
    printf("%s: [%.6f, %.6f, %.6f, %.6f]\n", name, q.w, q.x, q.y, q.z);
}
